/*
 * @Author: Zhaoq 1327153842@qq.com
 * @Date: 2022-08-02 14:05:45
 * @LastEditors: Zhaoq 1327153842@qq.com
 * @LastEditTime: 2022-08-02 14:12:25
 * @FilePath: /fox_ws/src/fox_kinematics/include/kinematics.h
 * @Gitee: https://gitee.com/Vader-LI
 */
#ifndef _REINOVO_KINEMATICS_H
#define _REINOVO_KINEMATICS_H

#include <iostream>
#include <string>
#include <vector>
#include <complex>
const double PI = std::acos(-1);

#define default_kinematics_mode (2)

#define default_wheel_radius (0.25)
#define default_wheel_separation (0.5)
#define default_vx (0.5)
#define default_vy (0.5)
#define default_vth (1.0)


namespace reinovo
{
class ReinovoKinematics
{
public:
    enum MotionModel{
        kNone = 1,
        kDiff,
        kThreeWheeledOmni,
        kFourMecanumOmni,
        kFourWheelSlip,
        kFourWheeledOmni,
        kAckermannSteering
    };
    struct CarData
    {
        int car_mode;
        double wheel_separation_x;//x方向轮间距
        double wheel_separation_y;//y方向轮间距
        double wheel_radius;
        double max_x_vel;
        double max_y_vel;
        double max_theta_vel;
        /* data */
    };
    
    ReinovoKinematics();

    std::vector<double> ForwardKinematics(std::vector<double> motor_speed);
    std::vector<double> InverseKinematics(double vx, double vy, double vth);
    CarData car_data_;
};
}

#endif
